/*

 * Kalman.h
 *
 *  Created on: 30 Nov 2012
 *      Author: Daniel Sun


#ifndef KALMAN_H_
#define KALMAN_H_

#include "../hardware.h"

class Kalman {

public:

	//Declare constructor function, sampletime in ms
	Kalman(int sampletime, Gyroscope *gyro, Magnetometer *mag);

	//Declare function to get the filtered angle
	float getAngle();

	void setGyroBias(float bias);
	void setMagVar(float var);

//Let Kalman inherit from Gyroscope and Magetometer so that can use their functions to calculate angle and rate

private:

//Declare variance quantities for process noise and magnetometer
	float Q_angle;
	float Q_bias;
	float R_measure;

//Declare main state vector variables
	float angle;
	float bias;
	float rate;


//Declare estimate covariance matrix, Kalman gain, etc.
	float P[2][2];
	float K[2];
	float y;
	float S;

//Declare Starter that is used later to set initial angle and rate
	int Starter;

// Sampletime in s
	float dt;

	Gyroscope *gyro;
	Magnetometer *mag;

};
//start_Kalman;  //Create object so that constructor function is enabled and the filter is initiated

#endif  KALMAN_H_
*/

/** Kalman.h
 *
 *  Created on: 30 Nov 2012
 *      Author: Daniel Sun*/


#ifndef KALMAN_H_
#define KALMAN_H_

#include "../Threads/global.h"
class Kalman {

public:

	//Declare constructor function, sampletime in ms
	Kalman();
	//Declare function to get the filtered angle
	float getAngle();
	void setGyroBias(float bias);
	void setMagVar(float var);

private:
//Declare variance quantities for process noise and magnetometer
//	float Q_angle;
//	float Q_bias;
//Declare main state vector variables
	float angle;
	float angle_rep;
	float angle_old;
	float rate;
	float oldAngleMag;
	float kalmanInput;
	int cnt;
//Declare estimate covariance matrix, Kalman gain, etc.
	float P;
	float K;
	float y;
	float S;
//Declare Starter that is used later to set initial angle and rate
	int Starter;
// Sampletime in s
	float dt;

};
//start_Kalman;  //Create object so that constructor function is enabled and the filter is initiated

#endif  //KALMAN_H_
